/*
 * File:   mcQuaternion.h
 * Author: thales
 *
 * Created on September 30, 2009, 5:33 PM
 */

#include "mcMatrix.h"

using namespace mc;

/**
 * @brief Construtor padrão, constroi uma matriz 4x4 com o valor passado
 *
 * @param Valor a ser setado em todas as posições
 */
template <class T>
matrix4<T>::matrix4( const T t ) {
    this->_00 = t; this->_01 = t; this->_02 = t; this->_03 = t;
    this->_10 = t; this->_11 = t; this->_12 = t; this->_13 = t;
    this->_20 = t; this->_21 = t; this->_22 = t; this->_23 = t;
    this->_30 = t; this->_31 = t; this->_32 = t; this->_33 = t;
}


/**
 * @brief Construtor padrão, constroi uma matriz identidade
 *
 * @param Valor a ser setado em todas as posições
 */
template <class T>
matrix4<T>::matrix4() {
    this->make_identity();
}


/**
 * @brief Construtor explicito, constroi uma matriz 3x3 com os valores passados
 *
 * @param __00 - Posição 00
 * @param __01 - Posição 01
 * @param __11 - Posição 11
 * @param __12 - Posição 12
 */
template <class T>
matrix4<T>::matrix4(
    const T __00, const T __01, const T __02, const T __03,
    const T __10, const T __11, const T __12, const T __13,
    const T __20, const T __21, const T __22, const T __23,
    const T __30, const T __31, const T __32, const T __33
)
: _00(__00), _01(__01), _02(__02), _03(__03),
  _10(__10), _11(__11), _12(__12), _13(__13),
  _20(__20), _21(__21), _22(__22), _23(__23),
  _30(__30), _31(__31), _32(__32), _33(__33)
{}


/**
 * @brief Construtor por cópia de vetor, constroi uma matriz 3x3 com o vetor passado
 *
 * @param m - Matriz a ser copiada
 */
template <class T>
matrix4<T>::matrix4( const T *m ) {
    this->set_data(m);
}


/**
 * @brief Construtor por cópia de matriz. Constrói a partir de uma matriz 2x2
 *
 * @param m - Matriz a ser copiada
 */
template <class T>
matrix4<T>::matrix4( const matrix2<T> &m ) {
    this->_00 = m._00; this->_01 = m._01; this->_02 = (T) 0; this->_03 = (T) 0;
    this->_10 = m._10; this->_11 = m._11; this->_12 = (T) 0; this->_13 = (T) 0;
    this->_20 = (T) 0; this->_21 = (T) 0; this->_22 = (T) 0; this->_23 = (T) 0;
    this->_30 = (T) 0; this->_31 = (T) 0; this->_32 = (T) 0; this->_33 = (T) 0;
}

/**
 * @brief Construtor por cópia de matriz.
 *
 * @param m - Matriz a ser copiada
 */
template <class T>
matrix4<T>::matrix4( const matrix3<T> &m ) {
    this->_00 = m._00; this->_01 = m._01; this->_02 = m._02; this->_03 = (T) 0;
    this->_10 = m._10; this->_11 = m._11; this->_12 = m._12; this->_13 = (T) 0;
    this->_20 = m._20; this->_21 = m._21; this->_22 = m._22; this->_23 = (T) 0;
    this->_30 = (T) 0; this->_31 = (T) 0; this->_32 = (T) 0; this->_33 = (T) 0;
}

/**
 * @brief Construtor por cópia de matriz. Constrói a partir de uma matriz 4x4
 *
 * @param m - Matriz a ser copiada
 */
template <class T>
matrix4<T>::matrix4( const matrix4<T> &m ) {
    this->_00 = m._00; this->_01 = m._01; this->_02 = m._02; this->_03 = m._03;
    this->_10 = m._10; this->_11 = m._11; this->_12 = m._12; this->_13 = m._13;
    this->_20 = m._20; this->_21 = m._21; this->_22 = m._22; this->_23 = m._23;
    this->_30 = m._30; this->_31 = m._31; this->_32 = m._32; this->_33 = m._33;
}


/**
 * @brief Acessa um elemento da matriz
 *
 * @param i - Acesso a linha
 * @param j - Acesso a coluna
 */
template <class T>
T matrix4<T>::get_element( int i, int j ) {
    assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
    return this->_array[this->get_col_count() * j + i];
}

/**
 * @brief Seta um elemento da matriz 3x3
 *
 * @param i - Linha
 * @param j - Coluna
 * @param e - Elemento a ser setado
 */
template <class T>
void matrix4<T>::set_element( int i, int j, T e ) {
    assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
    this->_array[this->get_col_count() * j + i] = e;
}


/**
 * @brief Seta o vetor com os dados da matriz 4x4
 *
 * @return Os dados da matriz
 */
template <class T>
void matrix4<T>::set_data( const T *array ) {
    for( register unsigned int i = 0; i < this->get_size(); i++) this->_array[i] = array[i];
}



/**
 * @brief Cria uma matriz identidade 4x4
 */
template <class T>
void matrix4<T>::make_identity() {
    this->_00 = (T) 1; this->_01 = (T) 0; this->_02 = (T) 0; this->_03 = (T) 0;
    this->_10 = (T) 0; this->_11 = (T) 1; this->_12 = (T) 0; this->_13 = (T) 0;
    this->_20 = (T) 0; this->_21 = (T) 0; this->_22 = (T) 1; this->_23 = (T) 0;
    this->_30 = (T) 0; this->_31 = (T) 0; this->_32 = (T) 0; this->_33 = (T) 1;
}

/**
 * @brief Operador de soma de matrizes 4x4
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix4<T>& matrix4<T>::operator + ( const matrix4<T> &m2 ) {
    matrix4<T> *mr = new matrix4<T>( (T) 0 );
    mr->_00 = this->_00 + m2._00;
    mr->_01 = this->_01 + m2._01;
    mr->_02 = this->_02 + m2._02;
    mr->_03 = this->_03 + m2._03;
    mr->_10 = this->_01 + m2._02;
    mr->_11 = this->_11 + m2._11;
    mr->_12 = this->_12 + m2._12;
    mr->_13 = this->_13 + m2._13;
    mr->_20 = this->_20 + m2._20;
    mr->_21 = this->_21 + m2._21;
    mr->_22 = this->_22 + m2._22;
    mr->_23 = this->_23 + m2._23;
    mr->_30 = this->_30 + m2._30;
    mr->_31 = this->_31 + m2._31;
    mr->_32 = this->_32 + m2._32;
    mr->_33 = this->_33 + m2._33;
    return *mr;
}


/**
 * @brief Operador de subtração de matrizes 4x4
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix4<T>& matrix4<T>::operator - ( const matrix4<T> &m ) {
    matrix4<T> *mr = new matrix4<T>( (T) 0 );
    mr->_00 = this->_00 - m._00;
    mr->_01 = this->_01 - m._01;
    mr->_02 = this->_02 - m._02;
    mr->_03 = this->_03 - m._03;
    mr->_10 = this->_01 - m._02;
    mr->_11 = this->_11 - m._11;
    mr->_12 = this->_12 - m._12;
    mr->_13 = this->_13 - m._13;
    mr->_20 = this->_20 - m._20;
    mr->_21 = this->_21 - m._21;
    mr->_22 = this->_22 - m._22;
    mr->_23 = this->_23 - m._23;
    mr->_30 = this->_30 - m._30;
    mr->_31 = this->_31 - m._31;
    mr->_32 = this->_32 - m._32;
    mr->_33 = this->_33 - m._33;
    return *mr;
}

/**
 * @brief Operador de multiplicação de matrizes 4x4
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix4<T>& matrix4<T>::operator * ( const matrix4<T> &m ) {
    matrix4<T> *mr = new matrix4<T>( (T) 0 );
    mr._00 = this->_00 * m._00 + this->_01 * m._10 + this->_02 * m._20 + this->_03 * m._30;
    mr._01 = this->_00 * m._01 + this->_01 * m._11 + this->_02 * m._21 + this->_03 * m._31;
    mr._02 = this->_00 * m._02 + this->_01 * m._12 + this->_02 * m._22 + this->_03 * m._32;
    mr._03 = this->_00 * m._03 + this->_01 * m._13 + this->_02 * m._23 + this->_03 * m._33;

    mr._10 = this->_10 * m._00 + this->_11 * m._10 + this->_12 * m._20 + this->_13 * m._30;
    mr._11 = this->_10 * m._01 + this->_11 * m._11 + this->_12 * m._21 + this->_13 * m._31;
    mr._12 = this->_10 * m._02 + this->_11 * m._12 + this->_12 * m._22 + this->_13 * m._32;
    mr._13 = this->_10 * m._03 + this->_11 * m._13 + this->_12 * m._23 + this->_13 * m._33;

    mr._20 = this->_20 * m._00 + this->_21 * m._10 + this->_22 * m._20 + this->_23 * m._30;
    mr._21 = this->_20 * m._01 + this->_21 * m._11 + this->_22 * m._21 + this->_23 * m._31;
    mr._22 = this->_20 * m._02 + this->_21 * m._12 + this->_22 * m._22 + this->_23 * m._32;
    mr._23 = this->_20 * m._03 + this->_21 * m._13 + this->_22 * m._23 + this->_23 * m._33;

    mr._30 = this->_30 * m._00 + this->_31 * m._10 + this->_32 * m._20 + this->_33 * m._30;
    mr._31 = this->_30 * m._01 + this->_31 * m._11 + this->_32 * m._21 + this->_33 * m._31;
    mr._32 = this->_30 * m._02 + this->_31 * m._12 + this->_32 * m._22 + this->_33 * m._32;
    mr._33 = this->_30 * m._03 + this->_31 * m._13 + this->_32 * m._23 + this->_33 * m._33;
    return *mr;
}

/**
 * @brief Operador de soma de matrizes 4x4
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix4<T>& matrix4<T>::operator += ( const matrix4<T> &m ) {
    this->_00 += m._00;
    this->_01 += m._01;
    this->_02 += m._02;
    this->_03 += m._03;
    this->_10 += m._02;
    this->_11 += m._11;
    this->_12 += m._12;
    this->_13 += m._13;
    this->_20 += m._20;
    this->_21 += m._21;
    this->_22 += m._22;
    this->_23 += m._23;
    this->_30 += m._30;
    this->_31 += m._31;
    this->_32 += m._32;
    this->_33 += m._33;
    return *this;
}


/**
 * @brief Operador de subtração de matrizes 4x4
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix4<T>& matrix4<T>::operator -= ( const matrix4<T> &m ) {
    this->_00 -= m._00;
    this->_01 -= m._01;
    this->_02 -= m._02;
    this->_03 -= m._03;
    this->_10 -= m._02;
    this->_11 -= m._11;
    this->_12 -= m._12;
    this->_13 -= m._13;
    this->_20 -= m._20;
    this->_21 -= m._21;
    this->_22 -= m._22;
    this->_23 -= m._23;
    this->_30 -= m._30;
    this->_31 -= m._31;
    this->_32 -= m._32;
    this->_33 -= m._33;
    return *this;
}


/**
 * @brief Operador de multiplicação de matrizes 4x4
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix4<T>& matrix4<T>::operator *= ( const matrix4<T> &m ) {
    matrix4<T> *mr = new matrix4<T>( (T) 0 );
    mr->_00 = m._00 * this->_00 + m._01 * this->_10 + m._02 * this->_20 + m._03 * this->_30;
    mr->_01 = m._00 * this->_01 + m._01 * this->_11 + m._02 * this->_21 + m._03 * this->_31;
    mr->_02 = m._00 * this->_02 + m._01 * this->_12 + m._02 * this->_22 + m._03 * this->_32;
    mr->_03 = m._00 * this->_03 + m._01 * this->_13 + m._02 * this->_23 + m._03 * this->_33;

    mr->_10 = m._10 * this->_00 + m._11 * this->_10 + m._12 * this->_20 + m._13 * this->_30;
    mr->_11 = m._10 * this->_01 + m._11 * this->_11 + m._12 * this->_21 + m._13 * this->_31;
    mr->_12 = m._10 * this->_02 + m._11 * this->_12 + m._12 * this->_22 + m._13 * this->_32;
    mr->_13 = m._10 * this->_03 + m._11 * this->_13 + m._12 * this->_23 + m._13 * this->_33;

    mr->_20 = m._20 * this->_00 + m._21 * this->_10 + m._22 * this->_20 + m._23 * this->_30;
    mr->_21 = m._20 * this->_01 + m._21 * this->_11 + m._22 * this->_21 + m._23 * this->_31;
    mr->_22 = m._20 * this->_02 + m._21 * this->_12 + m._22 * this->_22 + m._23 * this->_32;
    mr->_23 = m._20 * this->_03 + m._21 * this->_13 + m._22 * this->_23 + m._23 * this->_33;

    mr->_30 = m._30 * this->_00 + m._31 * this->_10 + m._32 * this->_20 + m._33 * this->_30;
    mr->_31 = m._30 * this->_01 + m._31 * this->_11 + m._32 * this->_21 + m._33 * this->_31;
    mr->_32 = m._30 * this->_02 + m._31 * this->_12 + m._32 * this->_22 + m._33 * this->_32;
    mr->_33 = m._30 * this->_03 + m._31 * this->_13 + m._32 * this->_23 + m._33 * this->_33;
    return *mr;
}


/**
 * @brief Transpõe a matriz 4x4
 */
template <class T>
void matrix4<T>::transpose() {
    matrix4<T> m(this->get_data());
    this->_01 = m._10;
    this->_02 = m._20;
    this->_03 = m._30;
    this->_10 = m._01;
    this->_12 = m._21;
    this->_13 = m._31;
    this->_20 = m._02;
    this->_21 = m._12;
    this->_23 = m._32;
    this->_30 = m._03;
    this->_31 = m._13;
    this->_32 = m._23;
}


/**
 * @brief Transpõe a matriz 4x4
 */
template <class T>
void matrix4<T>::transpose( matrix4<T> &m ) {
    /// Transpoẽ os dois elementos que serão modificados
    this->_01 = m._10;
    this->_02 = m._20;
    this->_03 = m._30;
    this->_10 = m._01;
    this->_12 = m._21;
    this->_13 = m._31;
    this->_20 = m._02;
    this->_21 = m._12;
    this->_23 = m._32;
    this->_30 = m._03;
    this->_31 = m._13;
    this->_32 = m._23;
}


/**
 * @brief Retorna o determinante da matriz 4x4
 *
 * @return O determinante da matriz
 */
template <class T>
T matrix4<T>::determinant() {
    matrix3<T> m1(
        _11, _12, _13,
        _21, _22, _23,
        _31, _32, _33
    );

    matrix3<T> m2(
        _01, _02, _03,
        _21, _22, _23,
        _31, _32, _33
    );

    matrix3<T> m3(
        _01, _02, _03,
        _11, _12, _13,
        _31, _32, _33
    );

    matrix3<T> m4(
        _01, _02, _03,
        _11, _12, _13,
        _21, _22, _23
    );

    T det1 = m1.determinant();
    T det2 = m2.determinant();
    T det3 = m3.determinant();
    T det4 = m4.determinant();

    return _00 * det1 - _10 * det2 + _20 * det3 - _30 * det4;
}


/**
 * @brief Retorna o vetor com os dados da matriz 3x3
 *
 * @return Os dados da matriz
 */
template <class T>
T* matrix4<T>::get_data() {
    return this->_array;
}

/**
 * @brief Calcula a inversa desta matriz
 *
 * @return true se a matriz tiver inversa e falso caso contrário
 */
template <class T>
bool matrix4<T>::inverse() {

    matrix4<T> m(this->get_data());

    T det = this->determinant();

    if( det <= (T) 0 ) return false;

    /// Calcula o inverso do determinante
    T det_i = ((T)1) / det;

    /// Monta as matriz de cofatores dos elementos
    matrix3<T> a00(
        _11, _12, _13,
        _21, _22, _23,
        _31, _32, _33
    );

    matrix3<T> a01(
        _10, _12, _13,
        _20, _22, _23,
        _30, _32, _33
    );

    matrix3<T> a02(
        _10, _11, _13,
        _20, _21, _23,
        _30, _31, _33
    );

    matrix3<T> a03(
        _10, _11, _12,
        _20, _21, _22,
        _30, _31, _32
    );


    matrix3<T> a10(
        _01, _02, _03,
        _21, _22, _23,
        _31, _32, _33
    );

    matrix3<T> a11(
        _00, _02, _03,
        _20, _22, _23,
        _30, _32, _33
    );

    matrix3<T> a12(
        _00, _01, _03,
        _20, _21, _23,
        _30, _31, _33
    );

    matrix3<T> a13(
        _00, _01, _02,
        _20, _21, _22,
        _30, _31, _32
    );

    matrix3<T> a20(
        _01, _02, _03,
        _11, _12, _13,
        _31, _32, _33
    );

    matrix3<T> a21(
        _00, _02, _03,
        _10, _12, _13,
        _30, _32, _33
    );

    matrix3<T> a22(
        _00, _01, _03,
        _10, _11, _13,
        _30, _31, _33
    );

    matrix3<T> a23(
        _00, _01, _02,
        _10, _11, _12,
        _30, _31, _32
    );

    matrix3<T> a30(
        _01, _02, _03,
        _11, _12, _13,
        _21, _22, _23
    );

    matrix3<T> a31(
        _00, _02, _03,
        _10, _12, _13,
        _20, _22, _23
    );

    matrix3<T> a32(
        _00, _01, _03,
        _10, _11, _13,
        _20, _21, _23
    );

    matrix3<T> a33(
        _00, _01, _02,
        _10, _11, _12,
        _20, _21, _22
    );

    /// Calcula o determinante das matrizes adjuntas
    T det00 = a00.determinant();
    T det01 = a01.determinant();
    T det02 = a02.determinant();
    T det03 = a03.determinant();

    T det10 = a10.determinant();
    T det11 = a11.determinant();
    T det12 = a12.determinant();
    T det13 = a13.determinant();

    T det20 = a20.determinant();
    T det21 = a21.determinant();
    T det22 = a22.determinant();
    T det23 = a23.determinant();

    T det30 = a30.determinant();
    T det31 = a31.determinant();
    T det32 = a32.determinant();
    T det33 = a33.determinant();

    /// Finalmente calcula o determinante final
    this->_00 =  det00 * det_i;
    this->_01 = -det01 * det_i;
    this->_02 =  det02 * det_i;
    this->_03 = -det03 * det_i;

    this->_10 =  det10 * det_i;
    this->_11 = -det11 * det_i;
    this->_12 =  det12 * det_i;
    this->_13 = -det13 * det_i;

    this->_20 =  det20 * det_i;
    this->_21 = -det21 * det_i;
    this->_22 =  det22 * det_i;
    this->_23 = -det23 * det_i;

    this->_30 =  det30 * det_i;
    this->_31 = -det31 * det_i;
    this->_32 =  det32 * det_i;
    this->_33 = -det33 * det_i;

    return true;
}

/**
 * @brief Calcula a inversa de uma matriz
 *
 * @return true se a matriz tiver inversa e falso caso contra
 */
template <class T>
bool matrix4<T>::inverse( matrix4<T> &m ) {

    matrix4<T> mr = new matrix4<T>(m);
    mr->inverse();
    *this = mr;

    return true;
}
